{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "909ef67b-ab58-4a30-8914-65c72a6635ea",
   "metadata": {},
   "source": [
    "# Steering along navigation route"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 45,
   "id": "8e6aaf37-1633-416d-835e-15abee30df54",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Hello there\n"
     ]
    }
   ],
   "source": [
    "print(\"Hello there\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 46,
   "id": "b90802ce-8ca7-4c6a-9386-f4c7a1ed1c4c",
   "metadata": {},
   "outputs": [],
   "source": [
    "import carla\n",
    "import time\n",
    "import numpy as np\n",
    "import math\n",
    "import sys\n",
    "import cv2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 47,
   "id": "4f4b8742-b61d-497b-884e-fceb487e2cb4",
   "metadata": {},
   "outputs": [],
   "source": [
    "#connect to simulator\n",
    "client = carla.Client('localhost', 2000)\n",
    "world = client.get_world()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 48,
   "id": "e093336d-8482-486e-b4ce-2e88db04a4e1",
   "metadata": {},
   "outputs": [],
   "source": [
    "#utility script for destruction, clear all the actors (basically undo or destroy the vehicle)\n",
    "for actor in world.get_actors().filter('*vehicle*'):\n",
    "    actor.destroy()\n",
    "#remove top and bottom qoutes to execute"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 49,
   "id": "1968d73d-9483-4f86-8868-2d5874f5ee9b",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "'\\nspectator = world.get_spectator()\\ntransform = spectator.get_transform()\\nspectator.set_transform(carla.Transform())\\n'"
      ]
     },
     "execution_count": 49,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "'''\n",
    "spectator = world.get_spectator()\n",
    "transform = spectator.get_transform()\n",
    "spectator.set_transform(carla.Transform())\n",
    "'''"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 50,
   "id": "9dcc63b9-1015-4af5-aa58-e6b991f61029",
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "\"\\n#print all the vehicles\\nfor bp in world.get_blueprint_library().filter('vehicle'):\\n    print(bp.id)\\n\\nvehicle.tesla.model3\\n\""
      ]
     },
     "execution_count": 50,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "'''\n",
    "#print all the vehicles\n",
    "for bp in world.get_blueprint_library().filter('vehicle'):\n",
    "    print(bp.id)\n",
    "\n",
    "vehicle.tesla.model3\n",
    "'''"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 51,
   "id": "627dd041-d17c-4dab-8f9e-b56ab815ddcb",
   "metadata": {},
   "outputs": [],
   "source": [
    "# get the spawn points of world\n",
    "spawn_points = world.get_map().get_spawn_points()\n",
    "start_point = spawn_points[0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 52,
   "id": "008761cd-9333-4a7f-8334-311ac4c9b0ae",
   "metadata": {},
   "outputs": [],
   "source": [
    "#look for a blue print of your desired car, i want tesla\n",
    "vehicle_bp = world.get_blueprint_library().filter('vehicle.tesla.model3')\n",
    "#print(vehicle_bp)\n",
    "\n",
    "#spawn the vehicle on start point\n",
    "vehicle = world.try_spawn_actor(vehicle_bp[0], start_point)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 53,
   "id": "c00180c1-7494-4272-8aaf-d67f6bc0194f",
   "metadata": {},
   "outputs": [],
   "source": [
    "# position and create camera and open a live stream of car\n",
    "CAMERA_POS_Z = 3\n",
    "CAMERA_POS_X = -5\n",
    "\n",
    "camera_bp = world.get_blueprint_library().find('sensor.camera.rgb')\n",
    "camera_bp.set_attribute('image_size_x', '1920')\n",
    "camera_bp.set_attribute('image_size_y', '1080')\n",
    "\n",
    "camera_init_trans = carla.Transform(carla.Location(z=CAMERA_POS_Z, x=CAMERA_POS_X))\n",
    "camera = world.spawn_actor(camera_bp, camera_init_trans, attach_to = vehicle)\n",
    "\n",
    "def camera_callback(image, data_dict):\n",
    "    data_dict['image'] = np.reshape(np.copy(image.raw_data), (image.height, image.width, 4))\n",
    "\n",
    "image_w = camera_bp.get_attribute('image_size_x').as_int()\n",
    "image_h = camera_bp.get_attribute('image_size_y').as_int()\n",
    "\n",
    "camera_data = {\n",
    "    'image': np.zeros((image_h, image_w, 4))\n",
    "}\n",
    "\n",
    "camera.listen(lambda image: camera_callback(image, camera_data))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 54,
   "id": "787950e8-3c57-4070-9a10-08bfc542a5d6",
   "metadata": {},
   "outputs": [],
   "source": [
    "# define constants \n",
    "\n",
    "#define speed constants\n",
    "PREFERRED_SPEED = 30\n",
    "SPEED_TRESHOLD = 2\n",
    "\n",
    "#adding parameters to display text to the image\n",
    "font = cv2.FONT_HERSHEY_SIMPLEX\n",
    "\n",
    "#defining the lines and font to display\n",
    "org = (30, 30) # this line will be used to show current speed\n",
    "org2 = (30, 50) # this line will be used for future steering angle\n",
    "org3 = (30, 70) # and another line for future telemetry outputs\n",
    "org4 = (30, 90) # and another line for future telemetry outputs\n",
    "org3 = (30, 110) # and another line for future telemetry outputs\n",
    "fontScale = 0.5\n",
    "color = (255, 255, 255)\n",
    "thickness = 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 55,
   "id": "8dc92543-7313-4945-9fa8-8521278fb075",
   "metadata": {},
   "outputs": [],
   "source": [
    "# function to miantain the speed\n",
    "def maintain_speed(s):\n",
    "    if s >= PREFERRED_SPEED:\n",
    "        return 0\n",
    "    elif s < PREFERRED_SPEED - SPEED_TRESHOLD:\n",
    "        return 0.8\n",
    "    else:\n",
    "        return 0.4"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 57,
   "id": "a23d09cf-8bfa-4255-8258-a9e0b44d2c34",
   "metadata": {},
   "outputs": [
    {
     "ename": "ModuleNotFoundError",
     "evalue": "No module named 'agents'",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mModuleNotFoundError\u001b[0m                       Traceback (most recent call last)",
      "Cell \u001b[1;32mIn[57], line 4\u001b[0m\n\u001b[0;32m      2\u001b[0m \u001b[38;5;28;01mimport\u001b[39;00m \u001b[38;5;21;01msys\u001b[39;00m\n\u001b[0;32m      3\u001b[0m sys\u001b[38;5;241m.\u001b[39mpath\u001b[38;5;241m.\u001b[39mappend(\u001b[38;5;124m'\u001b[39m\u001b[38;5;124mG:\u001b[39m\u001b[38;5;124m\\\u001b[39m\u001b[38;5;124mCARLA_0.9.14\u001b[39m\u001b[38;5;124m\\\u001b[39m\u001b[38;5;124mWindowsNoEditor\u001b[39m\u001b[38;5;124m\\\u001b[39m\u001b[38;5;124mPythonAPI\u001b[39m\u001b[38;5;124m\\\u001b[39m\u001b[38;5;124mcarla\u001b[39m\u001b[38;5;124m'\u001b[39m)\n\u001b[1;32m----> 4\u001b[0m \u001b[38;5;28;01mfrom\u001b[39;00m \u001b[38;5;21;01magents\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mnavigation\u001b[39;00m\u001b[38;5;21;01m.\u001b[39;00m\u001b[38;5;21;01mglobal_route_planner\u001b[39;00m \u001b[38;5;28;01mimport\u001b[39;00m GlobalRoutePlanner\n\u001b[0;32m      6\u001b[0m point_a \u001b[38;5;241m=\u001b[39m start_point\u001b[38;5;241m.\u001b[39mlocation\n\u001b[0;32m      8\u001b[0m \u001b[38;5;66;03m#draw the longest possible route\u001b[39;00m\n",
      "\u001b[1;31mModuleNotFoundError\u001b[0m: No module named 'agents'"
     ]
    }
   ],
   "source": [
    "#import route planner code from the simulation library path\n",
    "import sys\n",
    "sys.path.append('G:\\CARLA_0.9.14\\WindowsNoEditor\\PythonAPI\\carla')\n",
    "from agents.navigation.global_route_planner import GlobalRoutePlanner\n",
    "\n",
    "point_a = start_point.location\n",
    "\n",
    "#draw the longest possible route\n",
    "sampling_resolution = 2\n",
    "\n",
    "grp = GlobalRoutePlanner(world.get_map(), sampling_resolution)\n",
    "\n",
    "distance = 0\n",
    "for loc in spawn_points: \n",
    "    cur_route = grp.trace_route(point_a, loc.location)\n",
    "    if len(cur_route) > distance: \n",
    "        distance = len(cur_route)\n",
    "        route = cur_route\n",
    "\n",
    "#printing the route on map in the form of waypoints\n",
    "for waypoint in route:\n",
    "    world.debug.draw_string(waypoint[0].transform.location, 'O', draw_shadow=False, \n",
    "                            color = carla.Color(r=0, g=255, b=0), life_time = 120.0,\n",
    "                            persistent_lines = True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "id": "c7787e55-16fc-4f25-b305-0c579e00ef51",
   "metadata": {},
   "outputs": [],
   "source": [
    "cv2.namedWindow('RGB Camera',cv2.WINDOW_AUTOSIZE)\n",
    "cv2.imshow('RGB Camera',camera_data['image'])\n",
    "\n",
    "vehicle.set_autopilot(True)\n",
    "\n",
    "#main loop \n",
    "quit = False\n",
    "\n",
    "while True:\n",
    "    # Carla Tick\n",
    "    world.tick()\n",
    "    if cv2.waitKey(1) == ord('q'):\n",
    "        vehicle.set_autopilot(False)\n",
    "        vehicle.apply_control(carla.VehicleControl(throttle=0, \n",
    "                                                   steer=0,\n",
    "                                                  brake=1))\n",
    "        quit = True\n",
    "        break\n",
    "    image = camera_data['image']\n",
    "    \n",
    "    #get current speed\n",
    "    v = vehicle.get_velocity()\n",
    "    speed = round(3.6 * math.sqrt(v.x**2 + v.y**2 + v.z**2),0)\n",
    "    # now we add the speed to the window showing a camera mounted on the car\n",
    "    image = cv2.putText(image, 'Speed: '+str(int(speed))+' kmh', org2, \n",
    "                        font, fontScale, color, thickness, cv2.LINE_AA)\n",
    "\n",
    "    # show forward vector values (z dimension is ignored as we are not navigating up or down)\n",
    "    fwd_vector = vehicle.get_transform().get_forward_vector()\n",
    "    # now we overlay x and y\n",
    "    image = cv2.putText(image, 'Fwd_vec.x: '+str(round(fwd_vector.x,3)), org3, \n",
    "                        font, fontScale, color, thickness, cv2.LINE_AA)\n",
    "    image = cv2.putText(image, 'Fwd_vec.y: '+str(round(fwd_vector.y,3)), org4, \n",
    "                        font, fontScale, color, thickness, cv2.LINE_AA)\n",
    "\n",
    "    cv2.imshow('RGB Camera',image)\n",
    "\n",
    "#clean up\n",
    "cv2.destroyAllWindows()\n",
    "camera.stop()\n",
    "\n",
    "for sensor in world.get_actors().filter('*sensor*'):\n",
    "    sensor.destroy()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "id": "f70d3c0f-d0c2-47d9-a61f-749be0b213d6",
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'route' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "Cell \u001b[1;32mIn[20], line 1\u001b[0m\n\u001b[1;32m----> 1\u001b[0m world\u001b[38;5;241m.\u001b[39mdebug\u001b[38;5;241m.\u001b[39mdraw_string(\u001b[43mroute\u001b[49m[\u001b[38;5;241m25\u001b[39m][\u001b[38;5;241m0\u001b[39m]\u001b[38;5;241m.\u001b[39mtransform\u001b[38;5;241m.\u001b[39mlocation, \u001b[38;5;124m'\u001b[39m\u001b[38;5;124m25\u001b[39m\u001b[38;5;124m'\u001b[39m, draw_shadow\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mFalse\u001b[39;00m,\n\u001b[0;32m      2\u001b[0m         color\u001b[38;5;241m=\u001b[39mcarla\u001b[38;5;241m.\u001b[39mColor(r\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m0\u001b[39m, g\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m0\u001b[39m, b\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m255\u001b[39m), life_time\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m600.0\u001b[39m,\n\u001b[0;32m      3\u001b[0m         persistent_lines\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mTrue\u001b[39;00m)\n\u001b[0;32m      4\u001b[0m world\u001b[38;5;241m.\u001b[39mdebug\u001b[38;5;241m.\u001b[39mdraw_string(route[\u001b[38;5;241m50\u001b[39m][\u001b[38;5;241m0\u001b[39m]\u001b[38;5;241m.\u001b[39mtransform\u001b[38;5;241m.\u001b[39mlocation, \u001b[38;5;124m'\u001b[39m\u001b[38;5;124m50\u001b[39m\u001b[38;5;124m'\u001b[39m, draw_shadow\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mFalse\u001b[39;00m,\n\u001b[0;32m      5\u001b[0m         color\u001b[38;5;241m=\u001b[39mcarla\u001b[38;5;241m.\u001b[39mColor(r\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m0\u001b[39m, g\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m0\u001b[39m, b\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m255\u001b[39m), life_time\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m600.0\u001b[39m,\n\u001b[0;32m      6\u001b[0m         persistent_lines\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mTrue\u001b[39;00m)\n\u001b[0;32m      7\u001b[0m world\u001b[38;5;241m.\u001b[39mdebug\u001b[38;5;241m.\u001b[39mdraw_string(route[\u001b[38;5;241m75\u001b[39m][\u001b[38;5;241m0\u001b[39m]\u001b[38;5;241m.\u001b[39mtransform\u001b[38;5;241m.\u001b[39mlocation, \u001b[38;5;124m'\u001b[39m\u001b[38;5;124m75\u001b[39m\u001b[38;5;124m'\u001b[39m, draw_shadow\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mFalse\u001b[39;00m,\n\u001b[0;32m      8\u001b[0m         color\u001b[38;5;241m=\u001b[39mcarla\u001b[38;5;241m.\u001b[39mColor(r\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m0\u001b[39m, g\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m0\u001b[39m, b\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m255\u001b[39m), life_time\u001b[38;5;241m=\u001b[39m\u001b[38;5;241m600.0\u001b[39m,\n\u001b[0;32m      9\u001b[0m         persistent_lines\u001b[38;5;241m=\u001b[39m\u001b[38;5;28;01mTrue\u001b[39;00m)\n",
      "\u001b[1;31mNameError\u001b[0m: name 'route' is not defined"
     ]
    }
   ],
   "source": [
    "world.debug.draw_string(route[25][0].transform.location, '25', draw_shadow=False,\n",
    "        color=carla.Color(r=0, g=0, b=255), life_time=600.0,\n",
    "        persistent_lines=True)\n",
    "world.debug.draw_string(route[50][0].transform.location, '50', draw_shadow=False,\n",
    "        color=carla.Color(r=0, g=0, b=255), life_time=600.0,\n",
    "        persistent_lines=True)\n",
    "world.debug.draw_string(route[75][0].transform.location, '75', draw_shadow=False,\n",
    "        color=carla.Color(r=0, g=0, b=255), life_time=600.0,\n",
    "        persistent_lines=True)\n",
    "world.debug.draw_string(route[95][0].transform.location, '95', draw_shadow=False,\n",
    "        color=carla.Color(r=0, g=0, b=255), life_time=600.0,\n",
    "        persistent_lines=True)\n",
    "world.debug.draw_string(route[120][0].transform.location, '120', draw_shadow=False,\n",
    "        color=carla.Color(r=0, g=0, b=255), life_time=600.0,\n",
    "        persistent_lines=True)\n",
    "world.debug.draw_string(route[150][0].transform.location, '150', draw_shadow=False,\n",
    "        color=carla.Color(r=0, g=0, b=255), life_time=600.0,\n",
    "        persistent_lines=True)\n",
    "\n",
    "world.debug.draw_string(route[175][0].transform.location, '175', draw_shadow=False,\n",
    "        color=carla.Color(r=0, g=0, b=255), life_time=600.0,\n",
    "        persistent_lines=True)\n",
    "world.debug.draw_string(route[200][0].transform.location, '200', draw_shadow=False,\n",
    "        color=carla.Color(r=0, g=0, b=255), life_time=600.0,\n",
    "        persistent_lines=True)\n",
    "world.debug.draw_string(route[225][0].transform.location, '225', draw_shadow=False,\n",
    "        color=carla.Color(r=0, g=0, b=255), life_time=600.0,\n",
    "        persistent_lines=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "id": "34d3081e-b229-49a7-87c6-69c9368949f8",
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'route' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[1;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "Cell \u001b[1;32mIn[19], line 1\u001b[0m\n\u001b[1;32m----> 1\u001b[0m wp_vector \u001b[38;5;241m=\u001b[39m \u001b[43mroute\u001b[49m[\u001b[38;5;241m25\u001b[39m][\u001b[38;5;241m0\u001b[39m]\u001b[38;5;241m.\u001b[39mtransform\u001b[38;5;241m.\u001b[39mget_forward_vector()\n\u001b[0;32m      3\u001b[0m \u001b[38;5;66;03m# get the cordinates of the car\u001b[39;00m\n\u001b[0;32m      4\u001b[0m car_x \u001b[38;5;241m=\u001b[39m vehicle\u001b[38;5;241m.\u001b[39mget_transform()\u001b[38;5;241m.\u001b[39mlocation\u001b[38;5;241m.\u001b[39mx\n",
      "\u001b[1;31mNameError\u001b[0m: name 'route' is not defined"
     ]
    }
   ],
   "source": [
    "wp_vector = route[25][0].transform.get_forward_vector()\n",
    "\n",
    "# get the cordinates of the car\n",
    "car_x = vehicle.get_transform().location.x\n",
    "car_y = vehicle.get_transform().location.y\n",
    "\n",
    "wp_x = route[25][0].transform.location.x\n",
    "wp_y = route[25][0].transform.location.y\n",
    "\n",
    "print('waypoint 25 forward vector.x: ',wp_vector.x)\n",
    "print('waypoint 25 forward vector.y: ',wp_vector.y)\n",
    "print('waypoint 25 location x: ',wp_x)\n",
    "print('waypoint 25 location y: ',wp_y)\n",
    "print('car location x: ',car_x)\n",
    "print('car location y: ',car_y)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "id": "5c76dfc6",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "angle to way point:  32.87534712262566\n"
     ]
    }
   ],
   "source": [
    "# calculate the angle and illustrate it\n",
    "angle = math.degrees(math.atan((wp_y - car_y)/(wp_x - car_x)))\n",
    "print(\"angle to way point: \", angle)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "afd85cd4",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.0"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
